#include <math.h>
#include "search_border.h"

void Search_Bottom_Line_OTSU(uint8 imageInput[RESULT_HEIGHT][RESULT_WIDTH], uint8 Height, uint8 Width,
							 uint8 Bottonline, ImageInfo &image_info)
{
	// 寻找左边边界
	for (int Xsite = Width / 2; Xsite > 0; Xsite--)
		if (imageInput[Bottonline][Xsite] == 255 && imageInput[Bottonline][Xsite - 1] == 0)
		{
			image_info.ImageDeal[Bottonline].LeftBorder = Xsite; // 获取底边左边线
			break;
		}
	// 寻找右边边界
	for (int Xsite = Width / 2; Xsite < RESULT_WIDTH; Xsite++)
		if (imageInput[Bottonline][Xsite] == 255 && imageInput[Bottonline][Xsite + 1] == 0)
		{
			image_info.ImageDeal[Bottonline].RightBorder = Xsite; // 获取底边右边线
			break;
		}
}

void Search_Left_and_Right_Lines(uint8 imageInput[RESULT_HEIGHT][RESULT_WIDTH], uint8 Height, uint8 Width,
								 uint8 Bottonline, ImageInfo &image_info)
{
	// 前进方向像素为黑——向右（左）转
	// 前进方向像素为白，左前方像素为黑——向前走
	// 前进方向像素为白，且左（右）前方像素为白（墙角）——斜着走（x,y值均改变），且向左转
	/* 前进方向定义：
	 * 0(3) 0 1(0)
	 *  3      1
	 * 3(2) 2 2(1)
	 */
	/*寻左线坐标规则*/
	int Left_Rule[2][8] = {
		{0, -1, 1, 0, 0, 1, -1, 0},	 // {0,-1},{1,0},{0,1},{-1,0} 前，右，后，左
		{-1, -1, 1, -1, 1, 1, -1, 1} // {-1,-1},{1,-1},{1,1},{-1,1} 左前，右前，右后，左后
	};
	/*寻右线坐标规则*/
	int Right_Rule[2][8] = {
		{0, -1, 1, 0, 0, 1, -1, 0},	 // {0,-1},{1,0},{0,1},{-1,0} 前，右，后，左
		{1, -1, 1, 1, -1, 1, -1, -1} // {1,-1},{1,1},{-1,1},{-1,-1} 右前，右后，左后，左前
	};
	uint8 Left_Xsite = image_info.ImageDeal[Bottonline].LeftBorder; // 当前左边线X坐标
	uint8 Left_Ysite = Bottonline;									// 当前左边线Y坐标
	uint8 Left_Rirection = 0;										// 左边线前进方向
	uint8 Pixel_Left_Xsite = 0;										// 下一步左边线X坐标
	uint8 Pixel_Left_Ysite = Bottonline;							// 下一步左边线Y坐标

	uint8 Right_Xsite = image_info.ImageDeal[Bottonline].RightBorder; // 当前右边线X坐标
	uint8 Right_Ysite = Bottonline;									  // 当前右边线Y坐标
	uint8 Right_Rirection = 0;										  // 右边线前进方向
	uint8 Pixel_Right_Xsite = 0;									  // 下一步右边线X坐标
	uint8 Pixel_Right_Ysite = Bottonline;							  // 下一步右边线Y坐标

	uint8 Ysite = Bottonline;			// 当前行
	image_info.ImageStatus.OFFLine = 1; // 图像顶边，即出现路径的最高一行
	while (1)
	{
		if (Ysite >= Pixel_Left_Ysite && Ysite >= Pixel_Right_Ysite)
		{
			if (Ysite < image_info.ImageStatus.OFFLine)
			{
				image_info.ImageStatus.OFFLine = Ysite;
				break;
			}
			else
				Ysite--;
		}

		/*********左边巡线*******/
		if ((Pixel_Left_Ysite > Ysite) || Ysite == image_info.ImageStatus.OFFLine) // 右边扫线
		{
			// 计算前方坐标
			Pixel_Left_Xsite = Left_Xsite + Left_Rule[0][2 * Left_Rirection];
			Pixel_Left_Ysite = Left_Ysite + Left_Rule[0][2 * Left_Rirection + 1];

			// 若前方是黑色，右转
			if (imageInput[Pixel_Left_Ysite][Pixel_Left_Xsite] == 0)
				Left_Rirection = (Left_Rirection + 1) % 4;
			// 若前方是白色
			else
			{
				// 计算左前方坐标
				Pixel_Left_Xsite = Left_Xsite + Left_Rule[1][2 * Left_Rirection];
				Pixel_Left_Ysite = Left_Ysite + Left_Rule[1][2 * Left_Rirection + 1];
				// 若左前方为黑色，直走，前进方向不变
				if (imageInput[Pixel_Left_Ysite][Pixel_Left_Xsite] == 0)
				{
					Left_Xsite = Pixel_Left_Xsite;
					Left_Ysite = Pixel_Left_Ysite;
				}
				// 若左前方为白色，向左前方走，前进方向左转
				else
				{
					Left_Xsite = Left_Xsite + Left_Rule[1][2 * Left_Rirection];
					Left_Ysite = Left_Ysite + Left_Rule[1][2 * Left_Rirection + 1];
					Left_Rirection = (Left_Rirection + 3) % 4;
				}
				image_info.ImageDeal[Left_Ysite].LeftBorder = Left_Xsite;
			}
		}
		/*********右边巡线*******/
		if ((Pixel_Right_Ysite > Ysite) || Ysite == image_info.ImageStatus.OFFLine) // 右边扫线
		{
			/*计算前方坐标*/
			Pixel_Right_Xsite = Right_Xsite + Right_Rule[0][2 * Right_Rirection];
			Pixel_Right_Ysite = Right_Ysite + Right_Rule[0][2 * Right_Rirection + 1];
			// 若前方是黑色，左转
			if (imageInput[Pixel_Right_Ysite][Pixel_Right_Xsite] == 0)
				Right_Rirection = (Right_Rirection + 3) % 4;
			// 若前方是白色
			else
			{
				/*计算右前方坐标*/
				Pixel_Right_Xsite = Right_Xsite + Right_Rule[1][2 * Right_Rirection];
				Pixel_Right_Ysite = Right_Ysite + Right_Rule[1][2 * Right_Rirection + 1];
				// 若左前方为黑色，直走，前进方向不变
				if (imageInput[Pixel_Right_Ysite][Pixel_Right_Xsite] == 0)
				{
					Right_Xsite = Right_Xsite + Right_Rule[0][2 * Right_Rirection];
					Right_Ysite = Right_Ysite + Right_Rule[0][2 * Right_Rirection + 1];
				}
				// 若左前方为白色，向右前方走，前进方向右转
				else
				{
					Right_Xsite = Pixel_Right_Xsite;
					Right_Ysite = Pixel_Right_Ysite;
					Right_Rirection = (Right_Rirection + 1) % 4;
				}
				image_info.ImageDeal[Right_Ysite].RightBorder = Right_Xsite;
			}
		}
		// Ysite<80是为了防止底部是斑马线扫描结束 TODO 可以作为参数
		if (abs(Pixel_Right_Xsite - Pixel_Left_Xsite) < 3 && Ysite < 80)
		{
			image_info.ImageStatus.OFFLine = Ysite;
			break;
		}
		// TODO 75可以作为参数
		if (abs(image_info.ImageDeal[Ysite + 1].RightBorder - image_info.ImageDeal[Ysite + 1].LeftBorder) < 3 && Ysite < 75)
		{
			image_info.ImageStatus.OFFLine = Ysite;
			break;
		}
	}
}

void Search_Border_OTSU(uint8 imageInput[RESULT_HEIGHT][RESULT_WIDTH], uint8 Height, uint8 Width, uint8 Bottonline,
						ImageInfo &image_info)
{
	/*封左右边界处理*/
	image_info.ImageStatus.whitelines = 0;
	image_info.ImageStatus.OFFLine = 1;
	image_info.ImageStatus.whitelines_L = 0;
	image_info.ImageStatus.whitelines_R = 0;
	for (int Ysite = 0; Ysite < RESULT_HEIGHT; Ysite++)
	{
		imageInput[Ysite][image_info.ImageDeal[Ysite].Left_Sideline] = 0;
		imageInput[Ysite][image_info.ImageDeal[Ysite].Right_Sideline] = 0;
		image_info.ImageDeal[Ysite].LeftBorder = 0;
		image_info.ImageDeal[Ysite].Wide = 0;
		image_info.ImageDeal[Ysite].RightBorder = RESULT_WIDTH;
	}
	/*封上下边界处理*/
	for (int Xsite = 0; Xsite < RESULT_WIDTH; Xsite++)
	{
		imageInput[0][Xsite] = 0;
		imageInput[Bottonline + 1][Xsite] = 0;
	}

	/******获取底部左右边线*******/
	Search_Bottom_Line_OTSU(imageInput, Height, Width, Bottonline, image_info);

	/********获取左右边线*********/
	Search_Left_and_Right_Lines(imageInput, Height, Width, Bottonline, image_info);

	/********获取每一行边界基本信息*********/
	for (int Ysite = Bottonline; Ysite > image_info.ImageStatus.OFFLine + 1; Ysite--)
	{
		/*获取左右拟合中线*/
		// image_info.ImageDeal[Ysite].Center = image_info.ImageDeal[Ysite].LeftBorder + 22;
		image_info.ImageDeal[Ysite].Center = (image_info.ImageDeal[Ysite].LeftBorder + image_info.ImageDeal[Ysite].RightBorder) / 2;
		/*获取左边线类型*/
		image_info.ImageDeal[Ysite].Wide = image_info.ImageDeal[Ysite].RightBorder - image_info.ImageDeal[Ysite].LeftBorder;
		if (image_info.ImageDeal[Ysite].Wide > 70)
			image_info.ImageStatus.whitelines++;
		if (image_info.ImageDeal[Ysite].LeftBorder < 5)
			image_info.ImageStatus.whitelines_L++;
		if (image_info.ImageDeal[Ysite].RightBorder > RESULT_WIDTH - 6)
			image_info.ImageStatus.whitelines_R++;
	}
}